
//A.40 lsf.c 
    
   /****************************************************************** 
    
       iLBC Speech Coder ANSI-C Source Code 
    
       lsf.c  
    
       Copyright (c) 2001, 
       Global IP Sound AB. 
       All rights reserved. 
    
   ******************************************************************/ 
    
   #include <string.h> 
   #include <math.h> 
    
#include"iLBC_define.h"
    
   /*----------------------------------------------------------------* 
    *  conversion from lpc coefficients to lsf coefficients  
    *---------------------------------------------------------------*/ 
    
   void a2lsf(  
       float *freq,/* (o) lsf coefficients */ 
       float *a    /* (i) lpc coefficients */ 
   ){ 
       float steps[LSF_NUMBER_OF_STEPS] =  
           {(float)0.00635, (float)0.003175, (float)0.0015875,  
           (float)0.00079375}; 
       float step; 
       int step_idx; 
       int lsp_index;   
       float p[LPC_HALFORDER]; 
       float q[LPC_HALFORDER]; 
       float p_pre[LPC_HALFORDER]; 
       float q_pre[LPC_HALFORDER]; 
       float old_p, old_q, *old; 
       float *pq_coef;  
       float omega, old_omega; 
       int i; 
     
       float hlp, hlp1, hlp2, hlp3, hlp4, hlp5; 
    
       for (i = 0; i < LPC_HALFORDER; i++){ 
           p[i] = (float)-1.0 * (a[i + 1] + a[LPC_FILTERORDER - i]); 
           q[i] = a[LPC_FILTERORDER - i] - a[i + 1]; 
       } 
        
       p_pre[0] = (float)-1.0 - p[0]; 
       p_pre[1] = - p_pre[0] - p[1]; 
       p_pre[2] = - p_pre[1] - p[2]; 
       p_pre[3] = - p_pre[2] - p[3]; 
       p_pre[4] = - p_pre[3] - p[4]; 
       p_pre[4] = p_pre[4] / 2; 
        
       q_pre[0] = (float)1.0 - q[0]; 
       q_pre[1] = q_pre[0] - q[1]; 
       q_pre[2] = q_pre[1] - q[2]; 
       q_pre[3] = q_pre[2] - q[3]; 
       q_pre[4] = q_pre[3] - q[4]; 
       q_pre[4] = q_pre[4] / 2; 
        
       omega = 0.0; 
       old_omega = 0.0; 
    
       old_p = FLOAT_MAX; 
       old_q = FLOAT_MAX; 
        
       /* Here we loop through lsp_index to find all the  
          LPC_FILTERORDER roots for omega. */   
    
       for (lsp_index = 0; lsp_index < LPC_FILTERORDER; lsp_index++){ 
            
           /* Depending on lsp_index being even or odd, we  
           alternatively solve the roots for the two LSP equations. */ 
    
            
           if ((lsp_index & 0x1) == 0) { 
               pq_coef = p_pre; 
               old = &old_p; 
           } else { 
               pq_coef = q_pre; 
               old = &old_q; 
           } 
            
           /* Start with low resolution grid */ 
    
           for (step_idx = 0, step = steps[step_idx];  
               step_idx < LSF_NUMBER_OF_STEPS;){ 
                
               /*  cos(10piw) + pq(0)cos(8piw) + pq(1)cos(6piw) +  
               pq(2)cos(4piw) + pq(3)cod(2piw) + pq(4) */ 
    
               hlp = (float)cos(omega * TWO_PI); 
     
               hlp1 = (float)2.0 * hlp + pq_coef[0]; 
               hlp2 = (float)2.0 * hlp * hlp1 - (float)1.0 +  
                   pq_coef[1]; 
               hlp3 = (float)2.0 * hlp * hlp2 - hlp1 + pq_coef[2]; 
               hlp4 = (float)2.0 * hlp * hlp3 - hlp2 + pq_coef[3]; 
               hlp5 = hlp * hlp4 - hlp3 + pq_coef[4]; 
                
                
               if (((hlp5 * (*old)) <= 0.0) || (omega >= 0.5)){ 
                    
                   if (step_idx == (LSF_NUMBER_OF_STEPS - 1)){ 
                        
                       if (fabs(hlp5) >= fabs(*old)) { 
                           freq[lsp_index] = omega - step; 
                       } else { 
                           freq[lsp_index] = omega; 
                       }    
                        
                        
                       if ((*old) >= 0.0){ 
                           *old = (float)-1.0 * FLOAT_MAX; 
                       } else { 
                           *old = FLOAT_MAX; 
                       } 
    
                       omega = old_omega; 
                       step_idx = 0; 
                        
                       step_idx = LSF_NUMBER_OF_STEPS; 
                   } else { 
                        
                       if (step_idx == 0) { 
                           old_omega = omega; 
                       } 
    
                       step_idx++; 
                       omega -= steps[step_idx]; 
    
                       /* Go back one grid step */ 
    
                       step = steps[step_idx]; 
                   } 
               } else { 
                    
               /* increment omega until they are of different sign,  
               and we know there is at least one root between omega  
               and old_omega */ 
                   *old = hlp5; 
                   omega += step; 
               } 
           } 
       } 
    
     
       for (i = 0; i < LPC_FILTERORDER; i++) { 
           freq[i] = freq[i] * TWO_PI; 
       } 
   } 
    
   /*----------------------------------------------------------------* 
    *  conversion from lsf coefficients to lpc coefficients  
    *---------------------------------------------------------------*/ 
    
   void lsf2a(  
       float *a_coef,  /* (o) lpc coefficients */ 
       float *freq     /* (i) lsf coefficients */ 
   ){ 
       int i, j; 
       float hlp; 
       float p[LPC_HALFORDER], q[LPC_HALFORDER]; 
       float a[LPC_HALFORDER + 1], a1[LPC_HALFORDER], a2[LPC_HALFORDER]; 
       float b[LPC_HALFORDER + 1], b1[LPC_HALFORDER], b2[LPC_HALFORDER]; 
    
       for (i = 0; i < LPC_FILTERORDER; i++) { 
           freq[i] = freq[i] * PI2; 
       } 
    
       /* Check input for ill-conditioned cases.  This part is not  
       found in the TIA standard.  It involves the following 2 IF  
       blocks. If "freq" is judged ill-conditioned, then we first  
       modify freq[0] and freq[LPC_HALFORDER-1] (normally  
       LPC_HALFORDER = 10 for LPC applications), then we adjust  
       the other "freq" values slightly */ 
    
        
       if ((freq[0] <= 0.0) || (freq[LPC_FILTERORDER - 1] >= 0.5)){ 
    
            
           if (freq[0] <= 0.0) { 
               freq[0] = (float)0.022; 
           } 
    
            
           if (freq[LPC_FILTERORDER - 1] >= 0.5) { 
               freq[LPC_FILTERORDER - 1] = (float)0.499; 
           } 
    
           hlp = (freq[LPC_FILTERORDER - 1] - freq[0]) /  
               (float) (LPC_FILTERORDER - 1); 
    
           for (i = 1; i < LPC_FILTERORDER; i++) { 
               freq[i] = freq[i - 1] + hlp; 
           } 
       } 
        
       memset(a1, 0, LPC_HALFORDER*sizeof(float)); 
       memset(a2, 0, LPC_HALFORDER*sizeof(float)); 
     
       memset(b1, 0, LPC_HALFORDER*sizeof(float)); 
       memset(b2, 0, LPC_HALFORDER*sizeof(float)); 
       memset(a, 0, (LPC_HALFORDER+1)*sizeof(float)); 
       memset(b, 0, (LPC_HALFORDER+1)*sizeof(float)); 
            
       /* p[i] and q[i] compute cos(2*pi*omega_{2j}) and  
       cos(2*pi*omega_{2j-1} in eqs. 4.2.2.2-1 and 4.2.2.2-2.   
       Note that for this code p[i] specifies the coefficients  
       used in .Q_A(z) while q[i] specifies the coefficients used  
       in .P_A(z) */ 
    
       for (i = 0; i < LPC_HALFORDER; i++){ 
           p[i] = (float)cos(TWO_PI * freq[2 * i]); 
           q[i] = (float)cos(TWO_PI * freq[2 * i + 1]); 
       } 
        
       a[0] = 0.25; 
       b[0] = 0.25; 
        
       for (i = 0; i < LPC_HALFORDER; i++){ 
           a[i + 1] = a[i] - 2 * p[i] * a1[i] + a2[i]; 
           b[i + 1] = b[i] - 2 * q[i] * b1[i] + b2[i]; 
           a2[i] = a1[i]; 
           a1[i] = a[i]; 
           b2[i] = b1[i]; 
           b1[i] = b[i]; 
       } 
        
       for (j = 0; j < LPC_FILTERORDER; j++){ 
            
           if (j == 0) { 
               a[0] = 0.25; 
               b[0] = -0.25; 
           } else { 
               a[0] = b[0] = 0.0; 
           } 
            
           for (i = 0; i < LPC_HALFORDER; i++){ 
               a[i + 1] = a[i] - 2 * p[i] * a1[i] + a2[i]; 
               b[i + 1] = b[i] - 2 * q[i] * b1[i] + b2[i]; 
               a2[i] = a1[i]; 
               a1[i] = a[i]; 
               b2[i] = b1[i]; 
               b1[i] = b[i]; 
           } 
    
           a_coef[j + 1] = 2 * (a[LPC_HALFORDER] + b[LPC_HALFORDER]); 
       } 
    
       a_coef[0] = 1.0; 
   } 
    
    
     
